import roslib; roslib.load_manifest('sushi_smach')
import rospy
import smach
import smach_ros
import yaml
import tf
import geometry_msgs.msg
import actionlib

from collections import defaultdict
from geometry_msgs.msg import PoseStamped
from pr2_tasks.tasks import Tasks
from pr2_tasks.pickplace import PickPlace
from pr2_tasks.tableware_detection import TablewareDetection
from pr2_tasks.pickplace_definitions import PlaceGoal
from pr2_python import head, base
from pr2_python.world_interface import WorldInterface
from pr2_python.planning_scene_interface import get_planning_scene_interface
from pr2_python.arm_mover import ArmMover
from pr2_tasks.pickup_tableware import PickupTableware
from sushi_smach.GraspTurntable import GraspTurntable
import sushi_smach.msg

det = TablewareDetection()
res = det.detect_objects()

pickup_goals = []
listener = tf.TransformListener()

#return 'success'

for pg in res.pickup_goals:
  if pg.label  == 'cup':
    pickup_goals.append(pg)
for pg in res.pickup_goals:
  if pg.label == 'bowl':
    pickup_goals.append(pg)
for pg in res.pickup_goals:
  if pg.label == 'small_bowl':
    pickup_goals.append(pg)
for pg in res.pickup_goals:
  if pg.label == 'graspable':
    pickup_goals.append(pg)

pickup_goals = pickup_goals[0:2]
#self.world.res.pickup_goals = pickup_goals

x = 0
y = 0
z = 0

arm_mover = ArmMover()

if pickup_goals[0].label is 'graspable':
  for point in pickup_goals[0].target.cluster.points:
    x += point.x / len(pickup_goals[0].target.cluster.points)
    y += point.y / len(pickup_goals[0].target.cluster.points)
    z += point.z / len(pickup_goals[0].target.cluster.points)
  if y < 0:
    pickup_goals[0].arm_name = 'right_name'
    if len(pickup_goals) > 1:
      pickup_goals[1].arm_name = 'left_arm'
  else:
    pickup_goals[1].arm_name = 'left_arm'
    if len(pickup_goals) > 1:
      pickup_goals[1].arm_name = 'right_arm'
else:
  if pickup_goals[0].target.potential_models[0].pose.pose.position.y < 0:
    pickup_goals[0].arm_name = 'right_arm'
    if len(pickup_goals) > 1:
      pickup_goals[1].arm_name = 'left_arm'
  else:
    pickup_goals[0].arm_name = 'left_arm'
    if len(pickup_goals) > 1:
      pickup_goals[1].arm_name = 'right_arm'
  x = pickup_goals[0].target.potential_models[0].pose.pose.position.x
  y = pickup_goals[0].target.potential_models[0].pose.pose.position.y
  z = pickup_goals[0].target.potential_models[0].pose.pose.position.z
  print str(pickup_goals[0].target.potential_models[0].pose)
goal_pose = PoseStamped()
goal_pose.pose.position.x = x
goal_pose.pose.position.y = y + 0.15
goal_pose.pose.position.z = z
goal_pose.pose.orientation.w = 1.0
goal_pose.header.frame_id = '/torso_lift_link'

base = base.Base()

#try:
print x
print y 
print z
arm_name = None
if pickup_goals[0].arm_name is 'left_arm':
  arm_name = 'left_arm'
else:
  arm_name = 'right_arm'

print str(arm_name)

(trans,rot) = listener.lookupTransform('/base_link','/map',rospy.Time.now())
x = trans[0] + x
y = trans[1] + y
z = trans[2] + z
 
self.base.move_manipulable_pose(x, y, z)
self.pickplace.pickup(pickup_goals[0])
handle = arm_mover.move_to_goal(pickup_goals[0].arm_name, goal_pose) 
# except :
#   print str(e)
#   return 'failure'

# if len(pickup_goals) == 1:
#   return 'success'

if pickup_goals[1].label is 'graspable':
  for point in pickup_goals[1].target.cluster.points:
    x = point.x / len(pickup_goals[1].target.cluster.points)
    y = point.y / len(pickup_goals[1].target.cluster.points)
    z = point.z / len(pickup_goals[1].target.cluster.points)
else:
  x = pickup_goals[1].target.potential_models[0].pose.pose.position.x
  y = pickup_goals[1].target.potential_models[0].pose.pose.position.y
  z = pickup_goals[1].target.potential_models[0].pose.pose.position.z

goal_pose = PoseStamped()
goal_pose.pose.position.x = x   
goal_pose.pose.position.y = y + 0.15
goal_pose.pose.position.z = z
goal_pose.pose.orientation.w = 1.0
goal_pose.header.frame_id = '/torso_lift_link'

#try:
self.base.move_manipulable_pose(x, y, z, left=(pickup_goals[1].arm_name is 'left_arm'))
self.pickplace.pickup(pickup_goals[1])
handle = arm_mover.move_to_goal(pickup_goals[1].arm_name, goal_pose)

#except:
#   return 'failure'

# return 'success'